#ifndef MOTIONITF_H_
#define MOTIONITF_H_
// #include "motion.h"
// #include "nml_intf/zucpose.h"
// #include "zuc/nml_intf/zucpos.h"
#include "tp/tpItf.h"
#include "mot_priv.h"
#ifdef __cplusplus
extern "C" {
#endif
extern ZucPose zucmotGetCur_Tooloffset();
extern PmRpy zucmotGetCur_BaseOffset();
extern ZucPose zucmotGetCur_UserOffset();
void zucmotGet_trackO2world(PmHomogeneous* Pm);
// motion debug
extern int zucmotGet_toppqueuelen();
extern int zucmotGet_maxfwlen();
extern int zucmotGet_axesAccLimit(int axesIndex, double* Acclimit);
extern int zucmotGet_axesVelLimit(int axesIndex, double* Vellimit);
extern int zucmotGet_JointVelLimit(int jointIndex, double* vellimit);
extern int zucmotGet_JointAccLimit(int jointIndex, double* Acclimit);
extern int zucmotGet_JointPosLimit(int jointIndex, double* lowBound, double* upBound);
extern int zucmotGet_JointActPos(int jointIndex, double* pos);
extern int zucmotGet_JointTrqLimit(int jointIndex, double* trq);
extern int zucmotGet_JointCmdPos(int jointIndex, double* pos);
extern int zucmotGet_JointVelDir(int jointIndex, int* veldir);
// position torque fb
extern ZucPose zucmotStatGet_convyr_pos_fb();
extern void zucmotGetStat_jointTrqFb(double* torque);
extern double zucmotGet_maxFeedScale();

// motion_status
extern double zucmotStatGet_velScale();
extern double zucmotStatGet_momvelScale();
extern unsigned char zucmotStatGet_enables_new();
extern int zucmotStatGet_ReduceMode();
extern int zucmotStatGet_still_dec();

extern int zucmotStatSet_ftConfig(int index, FtConfig ftConfig);

extern int zucmotStatGet_percentageModeLevel();
extern int zucmotStatGet_threePositionEnableLimit();
extern int zucmotStatGet_tcpForceLimit();
extern int zucmotGet_IsStepping();

extern int zucmotGet_TotalNumAIO(char busType);
extern int zucmotGet_TotalNumDIO(char busType);

extern void zucmotSetRotaryUnlock(int jnum, int unlock);
extern int zucmotGetRotaryIsUnlocked(int jnum);
extern int zucmot_GetIOStatus(IODevType devtype, int di_Index, int* value);
extern int zucmot_SetDO(IODevType devtype, int di_Index, int value);
extern int zucmot_SetAO(IODevType devtype, int di_Index, float value);
extern double zucmot_GetCfgTcpVelLimit();
extern int
zucmot_GetSafeZoneImformation(int* autoEnable, int* openEnable, int* safezone_Enable, int* safezone_Dengrous, int* elbow_sphere_radius, SafeZone* safezone);
extern int zucmot_checkSafezone(SafeZone zone,
                                ZucPose curPose,
                                ZucPose cur_elbow_Pose,
                                int elbow_wrist_flag,
                                double R_elbow,
                                ZucPose current_user_offset,
                                int pre_flag,
                                checksafefalg_out* checksafefalg);
extern int zucmot_SetCheck_safezone_prediction();

extern int zucmot_GetTrajCycleTime(double* trajCycleTime);

extern int zucmot_set_status_jvel_limit(int is_limit, int index);

extern int zucmot_set_status_cpos_cmd(ZucPose cartPose);

extern int zucmot_set_status_cpos_desired(ZucPose cartPose);

extern int zucmot_get_status_cpos_cmd(ZucPose* cartPose);

extern int zucmot_get_status_cpos_fb(ZucPose* cartPose);

extern int zucmot_set_jointpos_cmd(double* jpos_cmd);

extern int zucmotGet_numJoints();

extern int zucmotconfig_robotmodel();

static tpInput_vtable_t zucmotTp_vtable = {.curTooloffset = zucmotGetCur_Tooloffset,
                                           .max_planning_queue_size = zucmotGet_toppqueuelen,
                                           .max_fw_len = zucmotGet_maxfwlen,
                                           .curBaseOffset = zucmotGetCur_BaseOffset,
                                           .curUserOffset = zucmotGetCur_UserOffset,
                                           .axesAccLimit = zucmotGet_axesAccLimit,
                                           .axesVelLimit = zucmotGet_axesVelLimit,
                                           .JointVelLimit = zucmotGet_JointVelLimit,
                                           .JointAccLimit = zucmotGet_JointAccLimit,
                                           .JointPosLimit = zucmotGet_JointPosLimit,
                                           .JointActPos = zucmotGet_JointActPos,
                                           .JointTrqLimit = zucmotGet_JointTrqLimit,
                                           .JointCmdPos = zucmotGet_JointCmdPos,
                                           .JointVelDir = zucmotGet_JointVelDir,
                                           .convyr_pos_fb = zucmotStatGet_convyr_pos_fb,
                                           .jointTrqFb = zucmotGetStat_jointTrqFb,
                                           .maxFeedScale = zucmotGet_maxFeedScale,
                                           .setftConfig = zucmotStatSet_ftConfig,
                                           .percentageModeLevel = zucmotStatGet_percentageModeLevel,
                                           .threePositionEnableLimit = zucmotStatGet_threePositionEnableLimit,
                                           .tcpForceLimit = zucmotStatGet_tcpForceLimit,
                                           .isStepping = zucmotGet_IsStepping,
                                           .velScale = zucmotStatGet_velScale,
                                           .momvelScale = zucmotStatGet_momvelScale,
                                           .enables_new = zucmotStatGet_enables_new,
                                           .ReduceMode = zucmotStatGet_ReduceMode,
                                           .still_dec = zucmotStatGet_still_dec,
                                           .TotalNumAIO = zucmotGet_TotalNumAIO,
                                           .TotalNumDIO = zucmotGet_TotalNumDIO,
                                           .RotaryUnlock = zucmotSetRotaryUnlock,
                                           .RotaryIsUnlocked = zucmotGetRotaryIsUnlocked,
                                           .IOStatus = zucmot_GetIOStatus,
                                           .SetDO = zucmot_SetDO,
                                           .SetAO = zucmot_SetAO,
                                           .CfgTcpVelLimit = zucmot_GetCfgTcpVelLimit,
                                           .track_pre_singular = track_pre_singular,
                                           .GetSafeZoneImformation = zucmot_GetSafeZoneImformation,
                                           .checkSafezone = zucmot_checkSafezone,
                                           .SetCheck_safezone_prediction = zucmot_SetCheck_safezone_prediction,
                                           .GetTrajCycleTime = zucmot_GetTrajCycleTime,
                                           .max_distance_stop = max_distance_stop,
                                           .set_status_jvel_limit = zucmot_set_status_jvel_limit,
                                           .set_status_cpos_cmd = zucmot_set_status_cpos_cmd,
                                           .set_status_cpos_desired = zucmot_set_status_cpos_desired,
                                           .get_status_cpos_cmd = zucmot_get_status_cpos_cmd,
                                           .get_status_cpos_fb = zucmot_get_status_cpos_fb,
                                           .set_jointpos_cmd = zucmot_set_jointpos_cmd,
                                           .zucmotGet_numJoints = zucmotGet_numJoints,
                                           .get_robot_model = zucmotconfig_robotmodel};
#ifdef __cplusplus
}
#endif
#endif